#include "vision_task.h"
#include "usb_task.h"
#include "SolveTrajectory.h"
#include "referee.h"
#include "data_exchange_center.h"
#include "user_lib.h"
#include <string.h>
#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include "gimbal_task.h"
#include <stdbool.h>
#include <math.h>
#include "cmsis_os.h"

#define VISION_SAFE_DELAY 10                                  // 视觉任务安全周期(ms)
#define MAX_BIAS_TIME 50                                     // 最大允许时延(ms)
#define MIN_BULLET_SPEED 5.0f                                 // 最小有效子弹速度(m/s)
#define DEFAULT_BULLET_SPEED 18.0f
#define MAX_BULLET_SPEED 30.0f                                // 最大有效子弹速度(m/s)
#define VALID_ANGLE_RANGE (M_PI / 2)                          // 有效角度范围(±90度)
#define CLAMP_ANGLE(ang) (fmodf((ang) + 5 * PI, 2 * PI) - PI) // 角度归一化[-π, π]
#define CLAMP(value, min, max) ((value) < (min) ? (min) : ((value) > (max) ? (max) : (value)))

ReceiveDataVision_s *VisionData = NULL;
struct SolveTrajectoryParams stc;
Imu_t *IMU_v = NULL;
SolveAns ans;

static KalmanFilter pitch_filter;
// 声明静态函数
static void sanitize_trajectory_params(struct SolveTrajectoryParams *params);

const gimbal_motor_t *gimbal_vis;

int BallisticsSolve(void);
void InitVisionData(void);
void vision_debug(void);

void vision_task(void const *argument)
{
    InitVisionData();

    while (1)
    {
        /*-----------------------------
        数据采集与预处理
        -----------------------------*/
        // 获取传感器数据（带保护）
        get_robot_shoot_speed(&stc.current_v);
        // 速度有效性验证
        if (stc.current_v < MIN_BULLET_SPEED || stc.current_v > MAX_BULLET_SPEED)
        {
            stc.current_v = MAX_BULLET_SPEED;
        }

        stc.current_pitch = -IMU_v->pitch;
        stc.current_yaw = IMU_v->yaw;

        /*-----------------------------
        视觉数据处理层
        -----------------------------*/
        // 视觉数据指针有效性验证
        if (!VisionData || !VisionData->data.tracking)
        {
            memset(&ans, 0, sizeof(SolveAns));
            continue;
        }

        // 数据时效性检查
        uint32_t current_time = HAL_GetTick();
        if (current_time - VisionData->time_stamp > MAX_BIAS_TIME)
        {
            memset(&ans, 0, sizeof(SolveAns));
            continue;
        }

        /*-----------------------------
        参数预处理与消毒
        -----------------------------*/
        memcpy(&stc.vis_data, &VisionData->data, sizeof(stc.vis_data));
        sanitize_trajectory_params(&stc);

        // 计算时延（带保护）
        stc.bias_time = current_time - VisionData->time_stamp;
        stc.bias_time = (stc.bias_time > MAX_BIAS_TIME) ? MAX_BIAS_TIME : stc.bias_time;

        /*-----------------------------
        弹道解算核心
        -----------------------------*/
        if (VisionData->data.tracking)
        {
            autoSolveTrajectory(&stc, &ans.pitch_v, &ans.yaw_v,
                                &ans.aim_x, &ans.aim_y, &ans.aim_z);

            // 滤波处理（带保护）
            if (isfinite(ans.pitch_v))
            {
                float dt_vis = VISION_TIME / 1000.0f;
                float filtered_pitch = Kalman_Update(&pitch_filter, ans.pitch_v,
                                                     (ans.pitch_v - pitch_filter.angle) / dt_vis,
                                                     dt_vis);
                ans.pitch_v = CLAMP_ANGLE(filtered_pitch);
            }
            else
            {
                ans.pitch_v = 0.0f;
            }
        }

        /*-----------------------------
        输出消毒处理
        -----------------------------*/
        // 角度范围约束
        ans.pitch_v = CLAMP_ANGLE(ans.pitch_v);
        ans.yaw_v = CLAMP_ANGLE(ans.yaw_v);

        // NaN/Inf过滤
        ans.pitch_v = isfinite(ans.pitch_v) ? ans.pitch_v : 0.0f;
        ans.yaw_v = isfinite(ans.yaw_v) ? ans.yaw_v : 0.0f;
        ans.aim_x = isfinite(ans.aim_x) ? ans.aim_x : 0.0f;
        ans.aim_y = isfinite(ans.aim_y) ? ans.aim_y : 0.0f;
        ans.aim_z = isfinite(ans.aim_z) ? ans.aim_z : 0.0f;
    }
}

void InitVisionData()
{

    memset(&ans, 0, sizeof(SolveAns));

    gimbal_vis = get_pitch_motor_point();
    /*********************************部分变量********************************* */
    IMU_v = DataExchangeCenterSubscribe(IMU_NAME);
    VisionData = GetVisionData();
    // 定义固定参数
    stc.k = 0.038;               // 0.092                  //弹道系数
    stc.bullet_type = BULLET_17; // 自身机器人类型
    stc.vis_data.dz = 0.0;
    stc.bias_time = 100;
    stc.s_bias = 0.193190;
    stc.z_bias = 0.0; // 0.402726;

    // 变化
    stc.current_v = 18;    // 当前弹速
    stc.current_pitch = 0; // 当前pitch
    stc.current_yaw = 0;   // 当前pitch

    stc.vis_data.xw = 3.0;
    stc.vis_data.yw = 0;
    stc.vis_data.zw = 1.5;

    stc.vis_data.vxw = 0;
    stc.vis_data.vyw = 0;
    stc.vis_data.vzw = 0;

    stc.vis_data.v_yaw = 0;
    stc.vis_data.tar_yaw = 0.09131;

    stc.vis_data.r1 = 0.0;
    stc.vis_data.r2 = 0.0;

    Kalman_Init(&pitch_filter, 0.0001f, 0.0003f, 0.05f, 0.1f);
}

SolveAns *GetSolveAns(void)
{
    return &ans;
}

bool get_tracking_status(void)
{
    return ans.tracking;
}

void vision_debug()
{
    /*********************************部分变量********************************* */
    // 定义固定参数
    stc.k = 0.038;               // 0.092                  //弹道系数
    stc.bullet_type = BULLET_17; // 自身机器人类型
    // stc.vis_data.dz = 0.0;
    // stc.bias_time = 100;
    // stc.s_bias = 0.193190;
    stc.z_bias = 0; // 0.7;//0.402726;

    // 变化
     stc.current_v = 199999;             //当前弹速
     stc.current_pitch = 2;          //当前pitch
     stc.current_yaw = 2;            //当前pitch

    // ROS坐标
     stc.vis_data.xw = 3.0;
     stc.vis_data.yw = 0;
     stc.vis_data.zw = 1.5;

    // ROS速度
     stc.vis_data.vxw = 0;
     stc.vis_data.vyw = 0;
     stc.vis_data.vzw = 0;

    // 目标yaw
     stc.vis_data.v_yaw = 0;
     stc.vis_data.tar_yaw = 0.09131;

    stc.vis_data.r1 = 1.0;
    // stc.vis_data.r2 = 1.0;
}

/****************************** 工具函数 ******************************/

// 弹道参数消毒
static void sanitize_trajectory_params(struct SolveTrajectoryParams *params)
{
    if (!params)
        return;

    // 弹道系数保护
    params->k = (params->k > 0.01f && params->k < 0.2f) ? params->k : 0.038f;

    // 偏置参数约束
    params->s_bias = fmaxf(params->s_bias, 0.0f);
    params->z_bias = CLAMP(params->z_bias, -1.0f, 1.0f);

    // 速度二次验证
    if (params->current_v < MIN_BULLET_SPEED)
    {
        params->current_v = DEFAULT_BULLET_SPEED;
    }
}
